/*!
 * Copyright (c) 2012
 * Automation and Robotics Lab (LARA) at University of Brasilia (UnB)
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     - Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     - Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     - Neither the name of the Automation and Robotics Lab (LARA) nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License LGPL as
 * published by the Free Software Foundation, either version 3 of the
 * License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU Lesser General Public License LGPL for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License LGPL along with this program.
 * If not, see <http://www.gnu.org/licenses/>.
 *
 *****************************************************************
 *
 *   ROS stack name: schunk
 *   ROS package name: schunk_high
 *
 *   Authors: André A. Geraldes, email: andregeraldes@lara.unb.br
 *            Murilo M. Marinho, email: murilomarinho@lara.unb.br
 *
 *****************************************************************/

/******************************************************************************
                                   INCLUDES
 *****************************************************************************/

// Schunk_high
#include <schunk_high/joint_velocity_controller.h>

const int kControllerNID = 0x702;
const int kReferenceSize = 0;

/******************************************************************************
                                  MAIN ROUTINE
 *****************************************************************************/

int main(int argc, char** argv)
{
    // Initialize ROS, specify name of node
    ros::init(argc, argv, "schunk_joint_velocity_controller");

    // Create JointVelocityController object
    JointVelocityController joint_controller;
    joint_controller.GetRobotDH();
    joint_controller.GetVelocityParameters();

    // Main loop
    // It has been implemented inside the CartesianPositionController class so that the member variables could be easily accessed
    joint_controller.Control();

    return 0;
}

/******************************************************************************
                                    CONSTRUCTORS AND DESTRUCTORS
 *****************************************************************************/

JointVelocityController::JointVelocityController(): CartesianController("joint_controller", kControllerNID, kReferenceSize)
{
    // Implementation of topics to subscribe
    subscriber_joint_references_ = references_nodehandle_.subscribe("/schunk_high/joint_controller/references", 1, &JointVelocityController::ReferencesCallback, this);

    // Implementation of service servers
    service_server_init_control_ = service_server_nodehandle_.advertiseService("schunk_high/joint_controller/init", &JointVelocityController::InitCallback, this);
    service_server_enable_control_ = service_server_nodehandle_.advertiseService("schunk_high/joint_controller/enable_control", &JointVelocityController::EnableCallback, this);
    service_server_disable_control_ = service_server_nodehandle_.advertiseService("schunk_high/joint_controller/disable_control", &JointVelocityController::DisableCallback, this);
}

JointVelocityController::~JointVelocityController()
{
    if (enabled_)
    {
        StopSchunk();
        DisconnectFromSchunk();
    }
}

/******************************************************************************
                                     PARAMETER LOADING FUNCTIONS
 *****************************************************************************/

bool JointVelocityController::GetVelocityParameters()
{
    bool error = false;

    // Saturation Velocity
    if (nodehandle_.hasParam("/schunk_high/joint_controller/saturation_velocity"))
    {
        nodehandle_.getParam("/schunk_high/joint_controller/saturation_velocity", saturation_velocity_);
    }
    else
    {
        ROS_ERROR_STREAM("Parameter saturation_velocity not set, shutting down node...");
        nodehandle_.shutdown();
        error = true;
    }

    if (error) return false;

    control_parameters_loaded_ = true;
    return true;
}

/******************************************************************************
                                      CONTROL LOOP FUNCTION
 *****************************************************************************/

void JointVelocityController::Control()
{
    ros::Rate loop_rate(sample_rate_);

    while (nodehandle_.ok())
    {
        service_server_queue_.callAvailable((ros::WallDuration()));

        if (enabled_)
        {
            references_queue_.callAvailable((ros::WallDuration()));
        }
    }
    loop_rate.sleep();
}


/******************************************************************************
                           SUBSCRIBED TOPIC CALLBACKS
 *****************************************************************************/

void JointVelocityController::ReferencesCallback(const schunk_msgs::JointVelocities::ConstPtr& msg)
{
    if (enabled_)
    {
        // Check the dimension of the received message
        if (static_cast<int>(msg->velocities.size()) != robot_->dofs)
        {
            ROS_ERROR_STREAM("References being sent to schunk_reference_velocities' topic is not size " << robot_->dofs);
            return;
        }

        for (int i = 0; i < robot_->dofs; i++)
        {
            publisher_velocity_message_.velocities[i] = msg->velocities[i];

            // Check if the received velocities satisfy the saturation_velocity limit
            if (publisher_velocity_message_.velocities[i] > saturation_velocity_)
                publisher_velocity_message_.velocities[i] = saturation_velocity_;
            else if (publisher_velocity_message_.velocities[i] < -saturation_velocity_)
                publisher_velocity_message_.velocities[i] = -saturation_velocity_;
        }

        // Send Message to schunk_low
        publisher_velocity_message_.header.stamp = ros::Time::now();
        publisher_velocity_message_.NID = controller_NID_;
        publisher_velocity_.publish(publisher_velocity_message_);
        publisher_queue_.callAvailable(ros::WallDuration(0));
    }
}

/******************************************************************************
                             SERVICE SERVER CALLBACKS
 *****************************************************************************/

bool JointVelocityController::InitCallback(schunk_high::Init::Request& req, schunk_high::Init::Response& resp)
{
    ROS_INFO_STREAM("JointVelocityController::InitCallback - DEBUG 1");

    // If node is already initialized, there is nothing left to do
    if (initialized_)
    {
        ROS_INFO_STREAM("JointVelocityController::InitCallback - node already initialized");
        return true;
    }

    else
    {
        // Check if all parameters have been set before initializing the node
        if (!control_parameters_loaded_)
        {
            ROS_ERROR_STREAM("JointVelocityController::InitCallback - Cannot initialize node! Parameters have not been set yet");
            return false;
        }

        // Connect to the schunk_low_control node
        if (!InitSchunk()) return false;

        initialized_ = true;
        enabled_ = false;

        return true;
    }
}

bool JointVelocityController::EnableCallback(schunk_high::Enable::Request& req, schunk_high::Enable::Response& resp)
{
    // Node cannont be enabled unless it is initialized
    if (!initialized_)
    {
        ROS_ERROR_STREAM("JointVelocityController::EnableCallback - node not initialized");
        return false;
    }

    // If node is already enabled, there is nothing left to do
    if (enabled_)
    {
        ROS_INFO_STREAM("JointVelocityController::InitCallback - node already enabled");
        return true;
    }

    if (!ConnectToSchunk()) return false;
    if (!RecoverSchunk())
    {
        DisconnectFromSchunk();
        return false;
    }
    if (!ChangeSchunkControlMode(CONTROL_MODE_VELOCITY))
    {
        DisconnectFromSchunk();
        return false;
    }

    enabled_ = true;
    return true;
}

bool JointVelocityController::DisableCallback(schunk_high::Disable::Request& req, schunk_high::Disable::Response& resp)
{
    bool error = false;

    if (!StopSchunk()) error = true;

    if (!DisconnectFromSchunk()) error = true;

    enabled_ = false;
    return !error;
}
